package edu.wpi.first.wpilibj.swarm;
import edu.wpi.first.wpilibj.Utility;

public class Balancer {
	private Drivetrain drivetrain;
	private ManualGyro xGyro;
	private ManualGyro yGyro;
	private long       endtime;
	private int        count;
	
	private static final double threshold = 3.5;
	
	public Balancer(Drivetrain drivetrain) {
		this.drivetrain = drivetrain;
		xGyro           = new ManualGyro(3);
		yGyro           = new ManualGyro(2);
		endtime         = 0;
		count           = 0;
	}
	
	private double sign(double x) {
		if (x > 0.0) return 1.0;
		if (x < 0.0) return -1.0;
		else         return 0.0;
	}
	
	void balance(double xin, double yin, double rotin, boolean fieldoriented) {
		if (endtime > 0) {
			if (Utility.getFPGATime() > endtime + 400000) {
				drivetrain.setInput(0.0, 0.0, 0.0, true);
			}
			return;
		}
		if (Math.abs(xGyro.getRate()) > threshold ||
		    Math.abs(yGyro.getRate()) > threshold) count++;
		else if (count > 0) count--;
		if (count > 3) {
			endtime = Utility.getFPGATime();
			drivetrain.setInput(-0.1*sign(xin), -0.1*sign(yin), 0.0, true);
			return;
		}
		drivetrain.setInput(xin*0.08, yin*0.08, rotin*0.1, fieldoriented);
	}

	void initbalance() {
		endtime = 0;
		count   = 0;
	}
}